#include "msfsf_node.hpp"
#include "config.hpp"

#include <commons/util/util.hpp>

#include "support-msfsf/predict_by_acc.hpp"
#include "support-msfsf/update_by_pos.hpp"


using namespace sros;

MsfsfNode::MsfsfNode(const std::string &name, const std::string &module)
    : Node(name, module)
{
    
}
    
RetBool MsfsfNode::init()
{
    std::string root = util::env_var("sros_ROOT");
    if (root.empty())
    {
        root = "/opt/sros";
    }
    std::string config_path = root + "/runner/location/msfsf/params/config.json";
    Config::ins().init(config_path);
    
    Config::Cfg cfg;
    auto ret = Config::ins().load(cfg);
    if (!ret.v)
    {
        return ret;
    }
    return {true};

    predict_ = std::make_shared<location::predict_by_acc>();
    update_  = std::make_shared<location::update_by_pos>();
    update_->set_P(predict_);

    return {true};
}

void MsfsfNode::run()
{
    predict_->run();
    
    // test
    update_->update();
}

void MsfsfNode::close()
{
    update_ = nullptr;

    predict_->close();
    predict_ = nullptr;
}

